clear all;
% draw virtual tube in 3D
%% path points
waypts=[-5 50 50 20 30;
    50 50 40 40 10;
    10 10 10 10 10];
%% min snap plan generator curve
% set init param
v0 = [0,0,0,0];
a0 = [0,0,0,0];
v1 = [0,0,0,0];
a1 = [0,0,0,0];
total_time=100;
n_order=6;
% time distribution
ts = arrangeT(waypts,total_time);
% generator curve planning
polys_x = minimum_snap_single_axis_simple(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),2);
polys_y = minimum_snap_single_axis_simple(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),2);
polys_z = minimum_snap_single_axis_simple(waypts(3,:),ts,n_order,v0(3),a0(3),v1(3),a1(3),2);
%% draw generator curve
fx=[];fx1=[];fx2=[];fy=[];fy1=[];fy2=[];fz=[];fz1=[];fz2=[];
for i=1:size(polys_x,2)
    tt = ts(i):1.1:ts(i+1);
    xx = polys_vals(polys_x,ts,tt,0);
    xx1 =polys_vals(polys_x,ts,tt,1);
    xx2 =polys_vals(polys_x,ts,tt,2);
    yy = polys_vals(polys_y,ts,tt,0);
    yy1 = polys_vals(polys_y,ts,tt,1);
    yy2 = polys_vals(polys_y,ts,tt,2);
    zz = polys_vals(polys_z,ts,tt,0);
    zz1 = polys_vals(polys_z,ts,tt,1);
    zz2 = polys_vals(polys_z,ts,tt,2); 
    fx=[fx xx];fx1=[fx1 xx1];fx2=[fx2 xx2];
    fy=[fy yy];fy1=[fy1 yy1];fy2=[fy2 yy2];
    fz=[fz zz];fz1=[fz1 zz1];fz2=[fz2 zz2];
end
figure(2),
plot3(fx(1,:),fy(1,:),fz(1,:),'--r','linewidth',2);
axis equal;
hold on;
f = [fx;fy;fz];
%% tube surface
% normal vector
ff1 = [fx1;fy1;fz1];
t=ff1./sum(ff1.^2,1).^0.5;
n=diff(t,1,2);
n=[n n(:,end)];
n = n./sum(n.^2,1).^0.5;
% abnormal vector
b = cross(t,n);
% radius
radius = 8;

for k = 1:length(n(1,:))-1
    if n(:,k)'*n(:,k+1)<0
        zero_index = k;
       break; 
    end
end
%% draw tube surface
fo_x = [];fo_y=[];fo_z=[];
for theta = 0:0.5:2*pi
    for k=1:zero_index
%         if k < 40
%             radius = 8;
%         elseif k<55
%             radius = 8*exp(-0.03*(k-40));
%         end
        radius = 8 * dmysigma(k,40,80,0.001);
        fo(:,k) = f(:,k) + radius*(n(:,k)*cos(theta) + b(:,k)*sin(theta));
    end
    
%     plot3(fo(1,:),fo(2,:),fo(3,:),'-b');
    fo_x=[fo_x; fo(1,10:zero_index)];
    fo_y=[fo_y; fo(2,10:zero_index)];
    fo_z=[fo_z; fo(3,10:zero_index)];
end
fo_x=[fo(1,10:zero_index);fo_x];
fo_y=[fo(2,10:zero_index);fo_y];
fo_z=[fo(3,10:zero_index);fo_z];
figure(2),
fs=surf(fo_x,fo_y,fo_z);
% fs.EdgeColor =[0.3 0.3 0.3];
fs.EdgeAlpha = 0.2;
fs.FaceAlpha = 0.5;
fs.FaceColor = [0 0.8 1];
axis equal;
axis off;
grid off;

circle_fox = fo_x(:,end);
circle_foy = fo_y(:,end);
circle_foz = fo_z(:,end);
circshift(circle_fox,7); 
circshift(circle_foy,7);
circshift(circle_foz,7);

fo_x = [];fo_y=[];fo_z=[];
for theta = pi:0.5:2*pi+pi
    fo = f + radius*(n*cos(theta) + b*sin(theta));
%     plot3(fo(1,:),fo(2,:),fo(3,:),'-b');
    fo_x=[fo_x; fo(1,zero_index+1:end)];
    fo_y=[fo_y; fo(2,zero_index+1:end)];
    fo_z=[fo_z; fo(3,zero_index+1:end)];
end
fo_x=[fo(1,zero_index+1:end);fo_x];
fo_y=[fo(2,zero_index+1:end);fo_y];
fo_z=[fo(3,zero_index+1:end);fo_z];
fo_x=[circle_fox fo_x];
fo_y=[circle_foy fo_y];
fo_z=[circle_foz fo_z];
fs=surf(fo_x,fo_y,fo_z);
fs.EdgeAlpha = 0.2;
fs.FaceAlpha = 0.5;
fs.FaceColor = [0 0.8 1];
axis equal;
axis off;
grid off;





